/*
Arm Tracker and Glove Demo by Noah Zerkin (noazark AT gmail.com) 2009
Released under GNU GPLv3.0 http://www.opensource.org/licenses/gpl-3.0.html
*/

#include <math.h>
#include "Wire.h"

#define outXhigh 0x29
#define outYhigh 0x2B
#define outZhigh 0x2D
#define statusReg 0x27
#define ID 0xC2
#define i2cID 0x1D

int mPin0 = 8;
int mPin1 = 7;
int mPin2 = 6;
int mPin3 = 5;

int enPin = 9;

long nextAccelRead = 30;
long time = 0;
long lastTime = 0;
long timeDelta = 0;
  byte x_val;
  byte y_val;
  byte z_val;

int gxPin = 0;
int gyPin = 2;
int flexPin = 1;
int sig = 3;
int vDiv = 10;

int axVal = 0;
int ayVal = 0;
int azVal = 0;
int gxVal = 0;
int gyVal = 0;
int flexVal = 0;

byte receiveBuffer[19];
byte readings[23];

void setup()
{
  pinMode(mPin0, OUTPUT);
  pinMode(mPin1, OUTPUT);
  pinMode(mPin2, OUTPUT);
  pinMode(mPin3, OUTPUT);
  pinMode(enPin, OUTPUT);
  pinMode(vDiv, OUTPUT);
  digitalWrite(vDiv, LOW);
  digitalWrite(enPin, LOW);
  plex(2);
  Serial.begin(57600);  
  Wire.begin(); // join i2c bus (address optional for master)
  Wire.beginTransmission(i2cID);
  Wire.send(0x21); // CTRL_REG2 (21h)
  Wire.send(B01000000);
  //SPI 4/3 wire
  //1=ReBoot - reset chip defaults
  //n/a
  //filter off/on
  //filter for freefall 2
  //filter for freefall 1
  //filter freq MSB
  //filter freq LSB - Hipass filter (at 400hz) 00=8hz, 01=4hz, 10=2hz, 11=1hz (lower by 4x if sample rate is 100hz)
  Wire.endTransmission();

  Wire.beginTransmission(i2cID);
  Wire.send(0x20); // CTRL_REG1 (20h)
  Wire.send(B01000111);
  //sample rate 100/400hz
  //power off/on
  //2g/8g
  //self test
  //self test
  //z enable
  //y enable
  //x enable
  Wire.endTransmission();
}

// -------------------------------------------------------------

void loop()
{
if(millis() >= nextAccelRead){
  accel();
}
displayReadings();
}

// -------------------------------------------------------------

void displayReadings()
{
  readings[0] = x_val;
  readings[1] = y_val;
  readings[2] = z_val;
  readings[3] = byte(constrain(map(analogRead(flexPin), 600, 1000, 0, 255),1, 254));
  readings[4] = byte(constrain(map(analogRead(gxPin), 300, 600, 0, 255),1, 254));
  readings[5] = byte(constrain(map(analogRead(gyPin), 300, 600, 0, 255),1, 254));
  muxRead();
  time = millis();
  readings[6] = time - lastTime; //timeDelta
  lastTime = time;
  serialOut();
//Serial.println(analogRead(flexPin));
//  serialOutASCII();
  delay(30);
}

void muxRead(){
  //digitalWrite(enPin, HIGH);
  digitalWrite(vDiv, HIGH);
  plex(0);delay(1);
  readings[7] = byte(map(analogRead(sig), 220, 272, 38, 217));
  plex(1);// delay(1);   //Accel Y
  readings[8] = byte(map(analogRead(sig), 410, 620, 38, 217));
  plex(2);// delay(1);  //Accel Z
  readings[9] = byte(map(analogRead(sig), 410, 610, 38, 217));// delay(1);
  digitalWrite(vDiv, LOW);
//  digitalWrite(enPin, LOW);
  for(int i = 3; i < 16; i+=1){
    plex(i);//delay(2); 
    readings[i+7] = byte(analogRead(sig));
  }
  plex(0);
}

void serialOut(){
//  Serial.print(ID, BYTE);
//  Serial.print(" ");
Serial.println();
  for(int i = 0; i < 24; i++){
    Serial.print(readings[i], BYTE);
  }
  Serial.println(165, BYTE);
}

void serialOutASCII(){
  Serial.print("a");
  Serial.print(" ");
  for(int i = 0; i < 23; i++){
    Serial.print(readings[i], DEC);
    Serial.print(" ");
  }
    Serial.println("b");
}
